#include "line_segment2d.h"

#include <algorithm>
#include <cmath>
#include <utility>

#include "math_utils.h"

namespace geometry
{
namespace
{

bool IsWithin(double val, double bound1, double bound2)
{
    if (bound1 > bound2) {
        std::swap(bound1, bound2);
    }
    return val >= bound1 - kMathEpsilon && val <= bound2 + kMathEpsilon;
}

} // namespace

LineSegment2d::LineSegment2d() { unit_direction_ = Vec2d(1, 0); }

LineSegment2d::LineSegment2d(const Vec2d &start, const Vec2d &end)
    : start_(start), end_(end)
{
    const double dx = end_.x() - start_.x();
    const double dy = end_.y() - start_.y();
    length_         = hypot(dx, dy);
    unit_direction_ =
        (length_ <= kMathEpsilon ? Vec2d(0, 0)
                                 : Vec2d(dx / length_, dy / length_));
    heading_ = unit_direction_.Angle();
}

Vec2d LineSegment2d::rotate(const double angle)
{
    Vec2d diff_vec = end_ - start_;
    diff_vec.SelfRotate(angle);
    return start_ + diff_vec;
}

double LineSegment2d::length() const { return length_; }

double LineSegment2d::length_sqr() const { return length_ * length_; }

// 计算点到线段的距离；
double LineSegment2d::DistanceTo(const Vec2d &point) const
{
    if (length_ <= kMathEpsilon) {
        return point.DistanceTo(start_);
    }
    const double x0 = point.x() - start_.x();
    const double y0 = point.y() - start_.y();
    // 计算point和起点连线在线段上的投影距离
    const double proj = x0 * unit_direction_.x() + y0 * unit_direction_.y();
    if (proj <= 0.0) {
        return hypot(x0, y0);
    }
    if (proj >= length_) {
        return point.DistanceTo(end_);
    }
    return std::abs(x0 * unit_direction_.y() - y0 * unit_direction_.x());
}

// 计算点到线段的距离；计算线段上到外部点最近的点，最多到端点；
double LineSegment2d::DistanceTo(const Vec2d &point,
                                 Vec2d *const nearest_pt) const
{
    //   CHECK_NOTNULL(nearest_pt);
    // bug
    if (nearest_pt == nullptr) {
        return 0;
    }
    if (length_ <= kMathEpsilon) {
        *nearest_pt = start_;
        return point.DistanceTo(start_);
    }
    const double x0   = point.x() - start_.x();
    const double y0   = point.y() - start_.y();
    const double proj = x0 * unit_direction_.x() + y0 * unit_direction_.y();
    if (proj < 0.0) {
        *nearest_pt = start_;
        return hypot(x0, y0);
    }
    if (proj > length_) {
        *nearest_pt = end_;
        return point.DistanceTo(end_);
    }
    *nearest_pt = start_ + unit_direction_ * proj;
    return std::abs(x0 * unit_direction_.y() - y0 * unit_direction_.x());
}

double LineSegment2d::DistanceSquareTo(const Vec2d &point) const
{
    if (length_ <= kMathEpsilon) {
        return point.DistanceSquareTo(start_);
    }
    const double x0   = point.x() - start_.x();
    const double y0   = point.y() - start_.y();
    const double proj = x0 * unit_direction_.x() + y0 * unit_direction_.y();
    if (proj <= 0.0) {
        return Square(x0) + Square(y0);
    }
    if (proj >= length_) {
        return point.DistanceSquareTo(end_);
    }
    return Square(x0 * unit_direction_.y() - y0 * unit_direction_.x());
}

double LineSegment2d::DistanceSquareTo(const Vec2d &point,
                                       Vec2d *const nearest_pt) const
{
    //   CHECK_NOTNULL(nearest_pt);
    // bug
    if (nearest_pt == nullptr) {
        printf("error");
        return 0;
    }
    if (length_ <= kMathEpsilon) {
        *nearest_pt = start_;
        return point.DistanceSquareTo(start_);
    }
    const double x0   = point.x() - start_.x();
    const double y0   = point.y() - start_.y();
    const double proj = x0 * unit_direction_.x() + y0 * unit_direction_.y();
    if (proj <= 0.0) {
        *nearest_pt = start_;
        return Square(x0) + Square(y0);
    }
    if (proj >= length_) {
        *nearest_pt = end_;
        return point.DistanceSquareTo(end_);
    }
    *nearest_pt = start_ + unit_direction_ * proj;
    return Square(x0 * unit_direction_.y() - y0 * unit_direction_.x());
}
// 判断点是否在线段上；
bool LineSegment2d::IsPointIn(const Vec2d &point) const
{
    if (length_ <= kMathEpsilon) {
        return std::abs(point.x() - start_.x()) <= kMathEpsilon &&
               std::abs(point.y() - start_.y()) <= kMathEpsilon;
    }
    const double prod = CrossProd(point, start_, end_);
    if (std::abs(prod) > kMathEpsilon) {
        return false;
    }
    return IsWithin(point.x(), start_.x(), end_.x()) &&
           IsWithin(point.y(), start_.y(), end_.y());
}
// 外部点与起点连线在线段方向上的投影距离；+ - 
double LineSegment2d::ProjectOntoUnit(const Vec2d &point) const
{
    return unit_direction_.InnerProd(point - start_);
}
// 计算外部点到线段直线的垂直距离；在线段右边的时候距离为负
double LineSegment2d::ProductOntoUnit(const Vec2d &point) const
{
    return unit_direction_.CrossProd(point - start_);
}
// 判断线段是否相交；计算线段的交点；
bool LineSegment2d::HasIntersect(const LineSegment2d &other_segment) const
{
    Vec2d point;
    return GetIntersect(other_segment, &point);
}
// 判断线段是否相交；计算线段的交点；
bool LineSegment2d::GetIntersect(const LineSegment2d &other_segment,
                                 Vec2d *const point) const
{
    //   CHECK_NOTNULL(point);
    // bug
    if (point == nullptr) {
        printf("error");
        return 0;
    }
    if (IsPointIn(other_segment.start())) {
        *point = other_segment.start();
        return true;
    }
    if (IsPointIn(other_segment.end())) {
        *point = other_segment.end();
        return true;
    }
    if (other_segment.IsPointIn(start_)) {
        *point = start_;
        return true;
    }
    if (other_segment.IsPointIn(end_)) {
        *point = end_;
        return true;
    }
    if (length_ <= kMathEpsilon || other_segment.length() <= kMathEpsilon) {
        return false;
    }
    const double cc1 = CrossProd(start_, end_, other_segment.start());
    const double cc2 = CrossProd(start_, end_, other_segment.end());
    if (cc1 * cc2 >= -kMathEpsilon) {
        return false;
    }
    const double cc3 =
        CrossProd(other_segment.start(), other_segment.end(), start_);
    const double cc4 =
        CrossProd(other_segment.start(), other_segment.end(), end_);
    if (cc3 * cc4 >= -kMathEpsilon) {
        return false;
    }
    const double ratio = cc4 / (cc4 - cc3);
    *point             = Vec2d(start_.x() * ratio + end_.x() * (1.0 - ratio),
                               start_.y() * ratio + end_.y() * (1.0 - ratio));
    return true;
}
// 计算点到线段所在直线的投影点；计算垂直距离
// return distance with perpendicular foot point.
double LineSegment2d::GetPerpendicularFoot(const Vec2d &point,
                                           Vec2d *const foot_point) const
{
    //   CHECK_NOTNULL(foot_point);
    // bug
    if (foot_point == nullptr) {
        printf("error");
        return 0;
    }

    if (length_ <= kMathEpsilon) {
        *foot_point = start_;
        return point.DistanceTo(start_);
    }
    const double x0   = point.x() - start_.x();
    const double y0   = point.y() - start_.y();
    const double proj = x0 * unit_direction_.x() + y0 * unit_direction_.y();
    *foot_point       = start_ + unit_direction_ * proj;
    return std::abs(x0 * unit_direction_.y() - y0 * unit_direction_.x());
}

// std::string LineSegment2d::DebugString() const
// {
//     return absl::StrCat("segment2d ( start = ", start_.DebugString(),
//                         "  end = ", end_.DebugString(), " )");
// }
} // namespace geometry
